#include "servo.h"


unsigned int servo_pos = 0 ;
unsigned int target_pos = 0 ;


void ConfigureTimer(void)
{
  //TACCR0	= 40000 ;
	TACCR0	= 30000 ;
  //TACCR1 = 2000 ; //closed
	TACCR1 = 1500 ;
  TACCTL0	|= CCIE ;
  TACCTL1	|= CCIE ;
  TACTL = TASSEL_2 | MC_1 | TACLR | ID_3 ;                  // TACLK = SMCLK/8, mode continuous
}


void enableServo(){
	SERVODIR |= SERVOPIN ;
	SERVOOUT &= ~SERVOPIN ;
	ConfigureTimer();
}

void disableServo(){
	TACTL &= ~ MC_3 ;
	  TACCTL0	&= ~CCIE ;
	  TACCTL1	&= ~CCIE ;
}

void setAngle(unsigned char pos){
	setPulse(POS_0 + ((unsigned int) pos * QUANTA));
}

void setPulse(unsigned int pulse){
	target_pos =  pulse;
	if(servo_pos == 0){
		servo_pos = target_pos ;
	}
}


#pragma vector=TIMER0_A0_VECTOR
__interrupt void Timer_A (void)
{

  SERVOOUT |= SERVOPIN ;
  int dif = target_pos - servo_pos ;
  int q ;
  if(dif != 0){
	  if(dif > 0){
	  	  if(dif < QUANTA * SPEED){
	  		  q = dif ;
	  	  }else{
	  		  q = QUANTA * SPEED;
	  	  }
	    }else{
	  	  if(dif > -(QUANTA * SPEED)){
	  		  q = dif ;
	  	  }else{
	  		q = -(QUANTA * SPEED);
	  	  }
	    }
	  servo_pos += q ;
  }
  TACCR1 = servo_pos ;
  TACCTL0 &= ~CCIFG;
  __bic_SR_register_on_exit(CPUOFF);
}

#pragma vector=TIMER0_A1_VECTOR
__interrupt void ta1_isr(void)
{
  SERVOOUT &= ~SERVOPIN ;
  TACCTL1 &= ~CCIFG;
  __bic_SR_register_on_exit(CPUOFF);
}
